perm filename TCOMP.SAI[OLD,HE] blob
sn#501020 filedate 1980-03-24 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00010 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 BEGIN "tcomp"
C00004 00003 ! Graph routine
C00008 00004 ! Now for the trajectory calculators: CUBSPL
C00012 00005 ! POLY - Matrix solvers: DECOMPOSE, SOLVE
C00018 00006 ! POLY, the polynomial spliner: The A matrix
C00026 00007 ! POLY continued: The B vectors
C00030 00008 ! TCALC - routine to set things up for POLY
C00034 00009 ! BSPLINE
C00040 00010 ! Driver loop
C00045 ENDMK
C⊗;
BEGIN "tcomp"
REQUIRE "DPYSUB.HDR[SUB,SYS]" SOURCE_FILE;
DEFINE CRLF="('15&'12)",
! = "COMMENT ",
TIL="STEP 1 UNTIL",
NTIL="STEP -1 UNTIL";
DEFINE MEM="MEMORY",
LOC="LOCATION",
MEMLOC(X,T)="MEMORY[LOCATION(X),T]";
define ttyset = "'047000400121";
INTEGER ARRAY DISPLY[1:'6000];
INTEGER nseg;
SAFE REAL ARRAY s[1:10,1:10];
DEFINE POS = 1; ! Position at this this knot;
DEFINE LSEG = 2; ! Length of time for this segment;
DEFINE TSEG = 3; ! Time this segment starts;
DEFINE VELOC = 4; ! Velocity (only at end points);
DEFINE C0 = 5, C1 = 6, C2 = 7, C3 = 8; ! Cubic coefficients;
DEFINE C4 = 9, C5 = 10; ! Higher order coefficients;
DEFINE RCLASS ="RECORD_CLASS",
RPTR = "RECORD_POINTER",
RANY = "RECORD_POINTER(ANY_CLASS)",
RNULL = "NULL_RECORD";
RCLASS TTHREAD (
REAL STIME;
RANY PLACE;
REAL ARRAY ANGLES, VELS; ! [LOJOINT:HIJOINT] = [1:1] here;
REAL ARRAY COEFF; ! [1:1,0:5]=[joint,degree] polynomial coefficients;
RPTR(TTHREAD) NEXT
);
REQUIRE "<><>" DELIMITERS;
DEFINE NewArray(type,name,bounds) = <
BEGIN
type ARRAY proxy bounds;
MEMORY[LOCATION(name)] ← MEMORY[LOCATION(proxy)];
MEMORY[LOCATION(proxy)] ← 0;
END>;
! Graph routine;
INTEGER POG;
PROCEDURE GRAPH (STRING des;INTEGER derv);
BEGIN "graph"
INTEGER I,J,ii;
REAL GDX,DX,DY,MAXV,MINV,x,y,t,st,dt;
SAFE REAL ARRAY data[0:200],c[0:5];
STRING COM2;
DEFINE X0 = -400; ! Graph orgin;
DEFINE Y0 = -260;
DEFINE NX = 800; ! Axis lengths;
DEFINE NY = 700;
dt ← (s[tseg,nseg+1] - s[tseg,1]) / 200;
j ← 1; ! sement pointer;
FOR ii ← 0 TIL 5 DO c[ii] ← s[c0+ii,j] / s[lseg,j]↑ii;
FOR i ← 0 TIL 200 DO
BEGIN
t ← i * dt + s[tseg,1];
IF t > s[tseg,j+1] THEN
BEGIN ! Time to move on to next segment;
j ← j + 1;
FOR ii ← 0 TIL 5 DO c[ii] ← s[c0+ii,j] / s[lseg,j]↑ii;
END;
st ← t - s[tseg,j];
data[i] ← CASE derv OF
( c[3]*st↑3 + c[2]*st↑2 + c[1]*st + c[0],
3*c[3]*st↑2 + 2*c[2]*st + c[1],
6*c[3]*st + 2*c[2] );
IF EQU(des,"BSPLNT") THEN ! Use higher order terms;
data[i] ← CASE derv OF
( c[5]*st↑5 + c[4]*st↑4 + data[i],
5*c[5]*st↑4 + 4*c[4]*st↑3 + data[i],
20*c[5]*st↑3 +12*c[4]*st↑2 + data[i] );
END;
MINV ← MAXV ← data[0];
FOR I ← 1 TIL 200 DO
IF (y←data[I]) > MAXV THEN MAXV←y ELSE IF y < MINV THEN MINV←y;
SETFORMAT(1,0);
DPYSET(DISPLY);
DX ← NX DIV (10 * (s[tseg,nseg+1] - s[tseg,1])); ! Scale the axes;
DY ← NY DIV (MAXV-MINV);
IF DY < 1 THEN DY ← NY/(MAXV-MINV);
MKSCALE(X0,Y0,DX,0,100,10*s[tseg,1],10*s[tseg,nseg+1]," Time"); ! Draw the axes;
COM2 ← " " & (CASE derv OF ("Position","Velocity","Acceleration"));
MKSCALE(X0,Y0,0,DY,40,MINV,MAXV,COM2);
GDX ← NX DIV 200; ! Scale for graphing;
AIVECT(X0,dy*(data[0]-MINV)+Y0); ! Graph it;
FOR I ← 1 TIL 200 DO AVECT(gdx*(I-1)+X0,dy*(data[I]-MINV)+Y0);
FOR J ← 2 TIL nseg DO ! Show where the knots are;
BEGIN
x ← X0 + 10 * (s[tseg,j] - s[tseg,1]) * dx;
FOR I ← 20 STEP 70 UNTIL NY DO
BEGIN
AIVECT(x,Y0+I);
RVECT(0,10)
END;
END;
DPYBIG(2);
AIVECT(-200,-316);
SETFORMAT(1,3);
DPYSST(des);
TYPLOC(-340,-500);
DPYOUT(POG);
quick_code
hrroi 1,['004000000120]; comment [004000,,"P"];
ttyset 1, ; ! this last stuff does an esc-P;
end;
END "graph";
! Now for the trajectory calculators: CUBSPL;
PROCEDURE CUBSPL;
BEGIN "cubspl"
SAFE REAL ARRAY tau[1:nseg+1],c[1:4,1:nseg+1];
INTEGER i,j,l,m,n;
REAL divdf1,divdf3,dtau,g;
n ← nseg+1;
FOR i ← 1 TIL n DO ! Set up tau & c;
BEGIN
tau[i] ← s[tseg,i];
c[1,i] ← s[pos,i];
END;
c[2,1] ← s[veloc,1]; ! Take care of velocitys at endpoints;
c[2,n] ← s[veloc,n];
! The following is the CUBSPL routine from de Boor's "A Practical Guide to Splines";
l ← n - 1;
FOR m ← 2 TIL n DO ! Compute first differences;
BEGIN
c[3,m] ← tau[m] - tau[m-1];
c[4,m] ← (c[1,m] - c[1,m-1]) / c[3,m];
END;
c[4,1] ← 1; ! Slope prescribed at left end;
c[3,1] ← 0;
IF n = 2 THEN ! Special case if no intermediate points;
BEGIN
c[2,1] ← (c[2,1] - c[3,1] * c[2,2]) / c[4,1];
dtau ← c[3,2];
divdf1 ← (c[1,2] - c[1,1]) / dtau;
divdf3 ← c[2,1] + c[2,2] - 2*divdf1;
c[3,1] ← (divdf1 - c[2,1] - divdf3) * dtau;
c[4,1] ← divdf3 * dtau;
c[2,1] ← c[2,1] * dtau;
FOR i ← 1 TIL 4 DO s[c0-1+i,1] ← c[i,1];
RETURN;
END;
FOR m ← 2 TIL l DO ! Generate interior knot eqns & forward pass of Gauss elim;
BEGIN
g ← -c[3,m+1] / c[4,m-1];
c[2,m] ← g * c[2,m-1] + 3 * (c[3,m] * c[4,m+1] + c[3,m+1] * c[4,m]);
c[4,m] ← g * c[3,m-1] + 2 * (c[3,m] + c[3,m+1]);
END;
FOR j ← l STEP -1 UNTIL 1 DO ! Carry out back substitution;
c[2,j] ← (c[2,j] - c[3,j] * c[2,j+1]) / c[4,j];
FOR i ← 2 TIL n DO ! Generate the cubic coefficents in each interval;
BEGIN
dtau ← c[3,i];
divdf1 ← (c[1,i] - c[1,i-1]) / dtau;
divdf3 ← c[2,i-1] + c[2,i] - 2*divdf1;
c[3,i-1] ← (divdf1 - c[2,i-1] - divdf3) * dtau;
c[4,i-1] ← divdf3 * dtau;
c[2,i-1] ← c[2,i-1] * dtau;
FOR j ← 1 TIL 4 DO s[c0-1+j,i-1] ← c[j,i-1]; ! Store result back in s array;
END;
END "cubspl";
! POLY - Matrix solvers: DECOMPOSE, SOLVE;
DEFINE DEBUG = <FALSE>;
SAFE OWN INTEGER ARRAY PS[1:50];
PROCEDURE DECOMPOSE(INTEGER N;SAFE REAL ARRAY A,LU);
! Both A and LU are [1:N, 1:N]. Uses global array PS.
Computes triangular matrices L and U and permutation matrix
PS so that LU=PA. Stores (L-I) and U both in LU. The call
DECOMPOSE(N,A,A) will overwrite A with LU.
;
BEGIN "decompose"
INTEGER I, J, K, PIVOTINDEX;
REAL NORMROW, PIVOT, SIZE, BIGGEST, MULT;
SAFE OWN REAL ARRAY R[1:50];
SIMPLE PROCEDURE ILOOP(INTEGER UL;REFERENCE REAL R1,R2);
! Machine-coded for efficiency;
START_CODE
LABEL LP,EU;
MOVE 1,-1('17);
MOVE 2,-2('17);
MOVE 3,-3('17);
SUB 3,K;
JUMPLE 3,EU;
LP: AOJ 1,;
AOJ 2,;
MOVN 4,MULT;
FMPR 4,(1);
FADRM 4,(2);
SOJG 3,LP;
EU: END;
IF N > 50
THEN USERERR(1,1,"DECOMPOSE can't handle a matrix as large as" & CVS(N));
! Initialize PS,LU and R;
FOR I←1 STEP 1 UNTIL N DO
BEGIN
PS[I]←I;
NORMROW←0;
FOR J←1 STEP 1 UNTIL N DO
BEGIN
LU[I,J]←A[I,J];
IF (NORMROW<ABS(LU[I,J])) THEN NORMROW←ABS(LU[I,J]);
END;
IF (NORMROW≠0)
THEN R[I]←1/NORMROW
ELSE BEGIN
R[I]←0;
USERERR(i,2,"Zero row in DECOMPOSE: ");
END;
END;
! Gaussian elimination with partial pivoting;
FOR K←1 STEP 1 UNTIL N-1 DO
BEGIN "kloop";
BIGGEST ← 0;
FOR I ← K STEP 1 UNTIL N DO
BEGIN
SIZE←ABS(LU[PS[I],K])*R[PS[I]];
IF (BIGGEST<SIZE)
THEN BEGIN
BIGGEST←SIZE;
PIVOTINDEX←I;
END;
END;
IF BIGGEST = 0
THEN BEGIN
USERERR(1,1,"Singular matrix in DECOMPOSE");
DONE "kloop";
END;
IF PIVOTINDEX ≠ K
THEN BEGIN
J←PS[K];
PS[K]←PS[PIVOTINDEX];
PS[PIVOTINDEX]←J;
END;
PIVOT←LU[PS[K],K];
FOR I←K+1 STEP 1 UNTIL N DO
BEGIN
LU[PS[I],K]←MULT←(LU[PS[I],K]/PIVOT);
IF MULT ≠ 0
THEN ILOOP(N,LU[PS[I],K],LU[PS[K],K]);
! The following is the result of the machine code:
FOR J ← K+1 STEP 1 UNTIL N DO
LU[PS[I],J]←LU[PS[I],J]-MULT*LU[PS[K],J];
END;
END "kloop";
IF (LU[PS[N],N]=0)
THEN USERERR(1,1,"Singular matrix in DECOMPOSE");
END "decompose";
SIMPLE PROCEDURE SOLVE(INTEGER N;SAFE REAL ARRAY LU,B,X);
! Arrays LU[1:N,1:N], B,X[1:N]. Uses global safe integer
array PS. Solves AX=B using LU from DECOMPOSE.
;
BEGIN "solve"
INTEGER I,J;
REAL DOT;
SIMPLE PROCEDURE ILOOP(INTEGER LL,UL;REFERENCE REAL R1,R2);
! Machine-coded for efficiency;
START_CODE
LABEL LP,EU;
MOVE 1,-1('17);
MOVE 2,-2('17);
MOVE 3,-3('17);
SUB 3,-4('17);
SETZ 4,;
JUMPL 3,EU;
LP: MOVE 5,(1);
FMPR 5,(2);
FADR 4,5;
AOJ 1,;
AOJ 2,;
SOJGE 3,LP;
EU: MOVEM 4,DOT;
END;
FOR I ← 1 STEP 1 UNTIL N DO
BEGIN
ILOOP(1,I-1,LU[PS[I],1],X[1]);
! Has this effect:
DOT←0
FOR J←1 STEP 1 UNTIL I-1 DO
DOT←DOT+LU[PS[I],J]*X[J];
X[I]←B[PS[I]]-DOT;
END;
X[N] ← X[N] / LU[PS[N],N];
FOR I ← N-1 STEP -1 UNTIL 1 DO
BEGIN ! RF: I changed loop upper index from N, to avoid
subscript errors;
ILOOP(I+1,N,LU[PS[I],I+1],X[I+1]);
! Has this effect:
DOT←0
FOR J←I+1 STEP 1 UNTIL N DO
DOT←DOT+LU[PS[I],J]*X[J];
X[I]←(X[I]-DOT)/LU[PS[I],I];
END;
END "solve";
! POLY, the polynomial spliner: The A matrix;
PROCEDURE POLY (RPTR(TTHREAD) FIRST, LAST; INTEGER LOJ, HIJ, NS);
! Calculate a trajectory for joints LOJ through HIJ using
the thread from FIRST to LAST. The number of segments in the
chunk is given by NS. The location for each node is to be
found in TTHREAD:ANGLES[*][JOINT], except for the
unconstrained points, which are distinguishable in that
TTHREAD:PLACE[*] = RNULL. The velocities of the first and
last points are given in TTHREAD:VELS[*][JOINT]. It is assumed
that the accelerations at these points are to be zero. The
timing for each segment is found in TTHREAD:STIME[*] in the
node at the end of the segment. The coefficients of the
resulting polynomial will be stored in the thread nodes, as
TTHREAD:COEFFS[*][JOINT,degree]. ;
BEGIN "poly"
DEFINE MEM(ARG) "<>" = <MEMORY[ARG,REAL]>;
SAFE REAL ARRAY A [1:4*NS,1:4*NS];
SAFE REAL ARRAY B, X [1:4*NS];
! A is a large matrix and B is a vector, for which we will
solve AX=B. A is the same for each joint, but B is
calculated anew for each joint. Thus only one call to
DECOMPOSE is needed;
RPTR(TTHREAD) P, Q; ! Used in tracking down the motion thread;
INTEGER ROW, COL, N, SEG, ALOC, I, JOINT; ! ALOC is used to point into A;
REAL TEMP;
ARRCLR(A);
N ← 4 * NS;
ROW ← COL ← 1;
! Compute the A matrix decomposition:;
! Fill the A matrix for the first segment;
ALOC ← LOCATION(A[1,1]);
A[ROW,COL] ← A[ROW+1,COL+1] ← 1.;
A[ROW+2,COL+2] ← 2.;
ROW ← ROW + 3;
COL ← COL + 4;
ALOC ← ALOC + 3*N + 4;
Q ← TTHREAD:NEXT[FIRST];
P ← TTHREAD:NEXT[Q];
FOR SEG ← 2 STEP 1 UNTIL NS DO
BEGIN "asegpol"
! Look at segment twixt Q and P;
IF TTHREAD:PLACE[Q] = RNULL
THEN BEGIN "auncnst" ! Left of segment is unconstrained;
MEM(ALOC) ← 1.;
MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ←
-1.;
ALOC ← ALOC + N;
MEM(ALOC-3) ← -1.;
MEM(ALOC-2) ← -2.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+1) ← TEMP ← TTHREAD:STIME[Q]/TTHREAD:STIME[P];
ALOC ← ALOC + N;
MEM(ALOC-2) ← -1.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+2) ← TEMP*TEMP;
! Same effect as:
A[ROW,COL] ← 1.
A[ROW,COL-1] ← A[ROW,COL-2] ← A[ROW,COL-3]
← A[ROW,COL-4] ← A[ROW+1,COL-3]
← A[ROW+2,COL-2] ← -1.
A[ROW+1,COL-1] ← A[ROW+2,COL-1] ← -3.
A[ROW+1,COL-2] ← -2.
A[ROW+2,COL+2] ← (A[ROW+1,COL+1] ←
TTHREAD:STIME[Q]/TTHREAD:STIME[P]) ↑ 2;
ROW ← ROW + 3;
COL ← COL + 4;
ALOC ← ALOC + N + 4;
END "auncnst"
ELSE BEGIN "acnst" ! Left of segment is constrained;
MEM(ALOC-1) ← MEM(ALOC-2) ← MEM(ALOC-3) ← MEM(ALOC-4)
← MEM(ALOC+N) ← 1.;
ALOC ← ALOC + N + N;
MEM(ALOC-1) ← -3.;
MEM(ALOC-2) ← -2.;
MEM(ALOC-3) ← -1.;
MEM(ALOC+1) ← TEMP ← TTHREAD:STIME[Q] / TTHREAD:STIME[P];
ALOC ← ALOC + N;
MEM(ALOC-2) ← -1.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+2) ← TEMP*TEMP;
! Equivalent to:
A[ROW,COL-1] ← A[ROW,COL-2]
← A[ROW,COL-3] ← A[ROW,COL-4] ← A[ROW+1,COL] ← 1.
A[ROW+2,COL-3] ← A[ROW+3,COL-2] ← -1.
A[ROW+2,COL-1] ← A[ROW+3,COL-1] ← -3.
A[ROW+2,COL-2] ← -2.
A[ROW+3,COL+2] ← (A[ROW+2,COL+1] ←
TTHREAD:STIME[Q]/TTHREAD:STIME[P]) ↑ 2;
ROW ← ROW + 4;
COL ← COL + 4;
ALOC ← ALOC + N + 4;
END "acnst";
Q ← P;
P ← TTHREAD:NEXT[Q];
END "asegpol";
! Take care of the constraints at the final point;
COL ← COL - 4;
MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ← 1.;
ALOC ← ALOC + N;
MEM(ALOC-3) ← 1.;
MEM(ALOC-2) ← 2.;
MEM(ALOC-1) ← 3.;
ALOC ← ALOC + N;
MEM(ALOC-2) ← 2.;
MEM(ALOC-1) ← 6.;
! Equivalent to:
A[ROW,COL] ← A[ROW,COL+1] ← A[ROW,COL+2] ← A[ROW,COL+3]
← A[ROW+1,COL+1] ← 1.
A[ROW+1,COL+2] ← A[ROW+2,COL+2] ← 2.
A[ROW+1,COL+3] ← 3.
A[ROW+2,COL+3] ← 6.;
ROW ← ROW + 3;
COL ← COL + 4;
IF ROW ≠ COL ∨ ROW ≠ N + 1 THEN USERERR(1,1,"ERROR IN POLY");
IF DEBUG ! Debug is defined false. Use RAID to remove the
jump around this code if you want to see the matrices;
THEN BEGIN "adebug" ! Print out the matrix A;
INTEGER WIDTH, DIGITS;
OUTSTR(CRLF);
GETFORMAT(WIDTH,DIGITS);
SETFORMAT(5,2);
FOR ROW ← 1 STEP 1 UNTIL N DO
BEGIN
FOR COL ← 1 STEP 1 UNTIL N DO
OUTSTR(CVF(A[ROW,COL]));
OUTSTR(CRLF);
END;
OUTSTR(CRLF);
SETFORMAT(WIDTH,DIGITS);
END "adebug";
DECOMPOSE(N,A,A);
! POLY continued: The B vectors;
! For each joint, calculate B, solve X and stow away;
FOR JOINT ← LOJ STEP 1 UNTIL HIJ DO
BEGIN "bcalc"
ARRCLR(B);
ROW ← 1;
! Fill the B matrix for the first segment;
B[ROW] ← TTHREAD:ANGLES[FIRST][JOINT];
B[ROW+1] ← TTHREAD:STIME[FIRST] * TTHREAD:VELS[FIRST][JOINT];
! If we ever put in non-zero acceleration constraints:
B[ROW+2] ← TTHREAD:STIME[FIRST][JOINT]↑2 * TTHREAD:ACCS[FIRST][JOINT];
ROW ← ROW + 3;
Q ← TTHREAD:NEXT[FIRST];
P ← TTHREAD:NEXT[Q];
FOR SEG ← 2 STEP 1 UNTIL NS DO
BEGIN "bsegpol"
! Look at segment twixt Q and P;
IF TTHREAD:PLACE[Q] = RNULL
THEN BEGIN "buncnst" ! Left of segment is unconstrained;
ROW ← ROW + 3;
END "buncnst"
ELSE BEGIN "bcnst" ! Left of segment is constrained;
B[ROW] ← B[ROW+1] ← TTHREAD:ANGLES[Q][JOINT];
ROW ← ROW + 4;
END "bcnst";
Q ← P;
P ← TTHREAD:NEXT[Q];
END "bsegpol";
! Take care of the constraints at the final point;
B[ROW] ← TTHREAD:ANGLES[LAST][JOINT];
B[ROW+1] ← TTHREAD:STIME[LAST] * TTHREAD:VELS[LAST][JOINT];
! If we ever put in non-zero acceleration constraints:
B[ROW+2] ← TTHREAD:STIME[LAST]↑2 * TTHREAD:ACCS[LAST][JOINT];
ROW ← ROW + 3;
IF ROW ≠ N + 1 THEN USERERR(1,1,"ERROR IN POLY");
IF DEBUG ! Debug is defined false. Use RAID to remove the
jump around this code if you want to see the matrices;
THEN BEGIN "bdebug" ! Print out the matrix B;
INTEGER WIDTH, DIGITS;
OUTSTR(CRLF);
GETFORMAT(WIDTH,DIGITS);
SETFORMAT(5,2);
OUTSTR(CRLF);
FOR ROW ← 1 STEP 1 UNTIL N DO
OUTSTR(CVF(B[ROW]));
OUTSTR(CRLF);
SETFORMAT(WIDTH,DIGITS);
END "bdebug";
SOLVE(N,A,B,X);
! Stow away the answer into the coefficient matrices;
P ← TTHREAD:NEXT[FIRST];
I ← 1;
FOR SEG ← 1 STEP 1 UNTIL NS DO
BEGIN "stow" ! Each iteration stores the coefficients into one node;
ARRBLT(TTHREAD:COEFF[P][JOINT,0],X[I],4);
I ← I + 4;
P ← TTHREAD:NEXT[P];
END "stow";
END "bcalc";
END "poly";
! TCALC - routine to set things up for POLY;
PROCEDURE TCALC;
BEGIN "tcalc"
INTEGER i,j;
REAL T1, T2, ST; ! Longest, next longest times;
RPTR(TTHREAD) Q1, Q2; ! Longest, next longest segment starters;
RPTR(TTHREAD) FIRSTTHREAD,CURTHREAD,P,Q;
T1 ← 0;
FOR i ← 1 TIL nseg+1 DO ! Copy s array into threads;
BEGIN
IF CURTHREAD = RNULL THEN FIRSTTHREAD ← CURTHREAD ← NEW_RECORD(TTHREAD)
ELSE CURTHREAD ← TTHREAD:NEXT[CURTHREAD] ← NEW_RECORD(TTHREAD);
st ← TTHREAD:STIME[CURTHREAD] ← s[tseg,i] - T1; ! Time since last segment;
T1 ← s[tseg,i];
TTHREAD:PLACE[CURTHREAD] ← CURTHREAD; ! So POLY knows we're constrained;
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[1:1,0:5]);
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[1:1]);
TTHREAD:ANGLES[CURTHREAD][1] ← s[pos,i];
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[1:1]);
TTHREAD:VELS[CURTHREAD][1] ← s[veloc,i] / st; ! we've already scaled it;
END;
T1 ← T2 ← 0.;
Q ← FIRSTTHREAD;
P ← TTHREAD:NEXT[Q];
WHILE Q ≠ CURTHREAD DO
BEGIN "max" ! This loop finds the two longest intervals;
IF (ST←TTHREAD:STIME[P]) > T2
THEN IF ST > T1
THEN BEGIN ! New longest;
T2 ← T1;
T1 ← ST;
Q2 ← Q1;
Q1 ← Q;
END
ELSE BEGIN ! New next-longest;
T2 ← ST;
Q2 ← Q;
END;
Q ← P;
P ← TTHREAD:NEXT[P]
END "max";
P ← TTHREAD:NEXT[Q1];
Q ← TTHREAD:NEXT[Q1] ← NEW_RECORD(TTHREAD);
TTHREAD:NEXT[Q] ← P;
TTHREAD:STIME[Q] ← T1 * 0.001;
TTHREAD:STIME[P] ← T1 - TTHREAD:STIME[Q];
NewArray(REAL,TTHREAD:COEFF[Q],[1:1,0:5]);
P ← TTHREAD:NEXT[Q2];
Q ← TTHREAD:NEXT[Q2] ← NEW_RECORD(TTHREAD);
TTHREAD:NEXT[Q] ← P;
TTHREAD:STIME[Q] ← T2 * 0.001;
TTHREAD:STIME[P] ← T2 - TTHREAD:STIME[Q];
NewArray(REAL,TTHREAD:COEFF[Q],[1:1,0:5]);
POLY(FIRSTTHREAD,CURTHREAD,1,1,nseg+2); ! Now do it;
P ← TTHREAD:NEXT[FIRSTTHREAD];
i ← 1;
WHILE P ≠ RNULL DO ! Copy the coefficients back into s;
IF TTHREAD:PLACE[P] = RNULL THEN P ← TTHREAD:NEXT[P] ! Skip unconstrained pts;
ELSE BEGIN
FOR j ← 0 TIL 3 DO s[c0+j,i] ← TTHREAD:COEFF[P][1,j];
i ← i + 1;
P ← TTHREAD:NEXT[P]
END;
END "tcalc";
! BSPLINE;
PROCEDURE BSPLINE (INTEGER k);
BEGIN "BSPLINE"
INTEGER i,j,l,n,left,imx;
REAL saved,sum,term,taui,diff;
REAL ARRAY t,g,bcoef[1:30],a[1:20,1:20];
REAL ARRAY deltal,deltar,biatx,dbiatx[1:k],scratch[1:k,1:k];
PROCEDURE BSPLVB (INTEGER jhigh,left; REAL x; BOOLEAN dbp(FALSE));
BEGIN
INTEGER i,j,ii;
REAL saved,term;
biatx[1] ← 1;
FOR j ← 1 TIL K-1 DO
BEGIN
deltar[j] ← t[left+j] - x;
deltal[j] ← x - t[left+1-j];
saved ← 0;
FOR i ← 1 TIL j DO
BEGIN
term ← biatx[i] / (deltar[i] + deltal[j+1-i]);
biatx[i] ← saved + deltar[i] * term;
saved ← deltal[j+1-i] * term;
END;
biatx[j+1] ← saved;
IF dbp ∧ j = k-2 THEN ! Copy derivative terms;
BEGIN
dbiatx[1] ← -(k-1) * biatx[1] / (t[left+1] - t[left-k+1]);
FOR ii ← 2 TIL k-1 DO
dbiatx[ii] ← (k-1)*biatx[ii-1] / (t[left+ii-1] - t[left-k+ii])
- (k-1)*biatx[ii]/(t[left+ii] - t[left-k+ii+1]);
dbiatx[k] ← (k-1) * biatx[k-1]/ (t[left+k-1]-t[left]);
END
END
END;
l ← nseg + 1;
n ← k + l - 2;
FOR i ← 1 TIL k DO ! Set up knots;
BEGIN
t[i] ← s[tseg,1];
t[n+i] ← s[tseg,nseg+1];
END;
FOR i ← 2 TIL nseg DO t[i+k-1] ← s[tseg,i];
ARRCLR(a);
left ← k;
FOR i ← 1 TIL l DO ! Construct the l position interpolation equations;
BEGIN
taui ← s[tseg,i];
imx ← (i+k) MIN (n+1);
! Find left in the interval [i,i+k-1] s.t. t[left] ≤ taui < t[left+1];
left ← left MAX i;
IF taui < t[left] THEN PRINT("Big trouble in BSPLINT - taui < t[left]"&crlf);
WHILE taui > t[left+1] ∧ left < imx DO left ← left + 1;
IF taui > t[left+1] THEN PRINT("Big trouble in BSPLINT - taui > t[left+1]"&crlf);
BSPLVB(k,left,taui);
FOR j ← 1 TIL k DO a[i+1,left-k+j] ← biatx[j]; ! Copy b-spline values;
END;
! Add the endpoint velocity equations;
BSPLVB(k,k,s[tseg,1],true); ! First end;
FOR j ← 1 TIL k DO a[1,j] ← dbiatx[j]; ! Copy b-spline derivative values;
BSPLVB(k,n,s[tseg,nseg+1]-0.01*(s[tseg,nseg+1]-s[tseg,nseg]),true); ! Other end too;
FOR j ← 1 TIL k DO a[n,n-k+j] ← dbiatx[j]; ! Copy b-spline derivative values;
DECOMPOSE(n,a,a); ! Do the gaussian elimination;
FOR j ← 1 TIL l DO g[j+1] ← s[pos,j]; ! Position;
g[1] ← s[veloc,1]; ! & endpoint velocity constraints;
g[n] ← s[veloc,nseg+1];
SOLVE(n,a,g,bcoef); ! Backsolve for the b-spline coefficients;
l ← 0;
FOR left ← k TIL n DO ! Compute the pp representation - BSPLPP;
BEGIN
IF t[left+1] = t[left] THEN CONTINUE; ! Skip over multiple knots;
l ← l +1;
FOR i ← 1 TIL k DO scratch[i,1] ← bcoef[left-k+i];
FOR j ← 1 TIL k-1 DO
FOR i ← 1 TIL k-j DO
IF (diff ← t[left+i] - t[left+i-k+j]) > 0 THEN
scratch[i,j+1] ← ((scratch[i+1,j] - scratch[i,j]) / diff) * (k-j);
biatx[1] ← 1;
s[c0-1+k,l] ← scratch[1,k];
FOR j ← 1 TIL k-1 DO
BEGIN ! body of BSPLVB;
deltar[j] ← t[left+j] - t[left];
deltal[j] ← t[left] - t[left+1-j];
saved ← 0;
FOR i ← 1 TIL j DO
BEGIN
term ← biatx[i] / (deltar[i] + deltal[j+1-i]);
biatx[i] ← saved + deltar[i] * term;
saved ← deltal[j+1-i] * term;
END;
biatx[j+1] ← saved;
sum ← 0;
FOR i ← 1 TIL j+1 DO sum ← sum + biatx[i] * scratch[i,k-j];
s[c0-1+k-j,l] ← sum;
END;
FOR j ← 1 TIL k DO
s[c0+j,l] ← s[c0+j,l] * s[lseg,l] ↑ j / (CASE j-1 OF (1,2,6,24,120));
FOR j ← k+1 TIL 5 DO s[c0+j,l] ← 0.0;
END
END "BSPLINE";
! Driver loop;
STRING com,des;
REAL a,b,c,v0,v1,dp;
INTEGER i,j,k;
POG←GETPOG;
SETFORMAT(1,3);
k ← 4; ! Start BSPLNT with cubics;
nseg ← 2; ! Start it out with some data;
s[pos,1] ← 180.0; s[tseg,1] ← 1.0; s[lseg,1] ← 2.1;
s[pos,2] ← 50.0; s[tseg,2] ← 3.1; s[lseg,2] ← 0.7;
s[pos,3] ← 35.0; s[tseg,3] ← 3.8; s[lseg,3] ← 0.0;
cubspl; ! So graphing gives something reasonable;
WHILE TRUE DO
BEGIN
PRINT(crlf&"*");
CASE (INCHRW LOR '40) OF
BEGIN
["c"] BEGIN cubspl; des ← "CUBSPL"; graph(des,0) END;
["t"] BEGIN tcalc; des ← "TCALC"; graph(des,0) END;
["b"] BEGIN bspline(k); des ← "BSPLNT"; graph(des,0) END;
["k"] BEGIN
PRINT(" should be: ");
LODED(CVS(k)&crlf);
com ← INCHWL;
k ← INTSCAN(com,i);
END;
["n"] BEGIN ! get new data;
PRINT("ew data: # segs: ");
LODED(CVS(nseg)&crlf);
com ← INCHWL;
nseg ← INTSCAN(com,i);
PRINT("position time pairs:"&crlf);
FOR j ← 1 TIL nseg+1 DO
BEGIN
LODED(CVF(s[pos,j])&" "&CVF(s[tseg,j])&crlf);
com ← INCHWL;
s[pos,j] ← REALSCAN(com,i);
s[tseg,j] ← REALSCAN(com,i);
END;
FOR j ← 1 TIL nseg DO s[lseg,j] ← s[tseg,j+1] - s[tseg,j];
PRINT("initial & final velocities: ");
LODED(CVF(s[veloc,1])&" "&CVF(s[veloc,nseg+1])&crlf);
com ← INCHWL;
s[veloc,1] ← REALSCAN(com,i);
s[veloc,nseg+1] ← REALSCAN(com,i);
END;
["m"] BEGIN
PRINT("odify initial & final velocities: ");
LODED(CVF(s[veloc,1])&" "&CVF(s[veloc,nseg+1])&crlf);
com ← INCHWL;
s[veloc,1] ← REALSCAN(com,i);
s[veloc,nseg+1] ← REALSCAN(com,i);
END;
["r"] BEGIN
FOR i ← 1 TIL nseg+1 DO s[c0,i] ← s[pos,i];
FOR i ← 1 TIL nseg+1 DO s[pos,i] ← s[c0,nseg+2-i]; ! Positions swapped;
FOR i ← 2 TIL nseg DO s[c0,i] ← s[tseg,i] - s[tseg,1];
FOR i ← 2 TIL nseg DO s[tseg,i] ← s[tseg,nseg+1] - s[c0,nseg+2-i]; ! time ok;
FOR i ← 1 TIL nseg DO s[lseg,i] ← s[tseg,i+1] - s[tseg,i];
a ← -s[veloc,1];
s[veloc,1] ← -s[veloc,nseg+1];
s[veloc,nseg+1] ← a;
END;
["p"] GRAPH(des,0);
["v"] GRAPH(des,1);
["a"] GRAPH(des,2);
["d"] BEGIN
quick_code
hrroi 1,['004000000516]; comment [004000,,'400+"N"];
ttyset 1, ; ! this last stuff does a brk-N;
end;
FOR i ← 1 TIL nseg DO
BEGIN
PRINT(crlf,i, " : ");
IF EQU(des,"BSPLNT") THEN PRINT(s[c5,i]," ",s[c4,i]," ");
FOR j ← 3 NTIL 0 DO PRINT(s[c0+j,i]," ");
END;
END;
["e"] DONE;
ELSE print("?")
END;
END;
quick_code
hrroi 1,['004000000516]; comment [004000,,'400+"N"];
ttyset 1, ; ! this last stuff does a brk-N;
end;
CALL(0,"EXIT");
END "tcomp";